#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"

// 右轮前进
#define GPIO10 10
// 右轮后退
#define GPIO9 9
// 左轮前进
#define GPIO1 1
// 左轮后退
#define GPIO0 0

#define GPIOFUNC 0

#define PWM_FREQ 1000                    // PWM频率1KHz (周期1ms)
 

// 实时速度检测
float forward_speed = 0;
#define MAX_SPEED_CHANGE 100

typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
    DIR_BIG_LEFT = 6,
    DIR_BIG_RIGHT = 7,
    DIR_BACK_LEFT=8,
    DIR_BACK_RIGHT=9,
}CarDir;
CarDir car_dir_index = DIR_STOP;
 

 
int speed_mode[11] = {0,100,200,300,400,500,600,700,800,900,1000
};

int speed_mode_index = 0;
int global_speed = 0;

void switch_speed(int flag) {
    if (flag == 0) {
        // 增加速度档位 相当于是减小速度
        speed_mode_index++;
        if (speed_mode_index >= sizeof(speed_mode) / sizeof(speed_mode[0])) {
            speed_mode_index = 0;  // 回到最慢
        }
    } else {
        // 减小速度档位 相当于增加速度
        if (speed_mode_index == 0) {
            speed_mode_index = sizeof(speed_mode) / sizeof(speed_mode[0]) - 1;  // 回到最快
        } else {
            speed_mode_index--;
        }
    }
    global_speed = speed_mode[speed_mode_index];
}
void control_speed(int num){
    global_speed=PWM_FREQ-num*(PWM_FREQ/100);
}

struct Gpio2PWM{
    unsigned int pwm_out;
    unsigned int pwm_port;
};

struct Gpio2PWM GS[11]={
    {HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3},
    {HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4},
    {HI_IO_FUNC_GPIO_2_PWM2_OUT,HI_PWM_PORT_PWM2},
    {HI_IO_FUNC_GPIO_3_PWM5_OUT,HI_PWM_PORT_PWM5},
    {HI_IO_FUNC_GPIO_4_PWM1_OUT,HI_PWM_PORT_PWM1},
    {HI_IO_FUNC_GPIO_5_PWM2_OUT,HI_PWM_PORT_PWM2},
    {HI_IO_FUNC_GPIO_6_PWM3_OUT,HI_PWM_PORT_PWM3},
    {HI_IO_FUNC_GPIO_7_PWM0_OUT,HI_PWM_PORT_PWM0},
    {HI_IO_FUNC_GPIO_8_PWM1_OUT,HI_PWM_PORT_PWM1},
    {HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0},
    {HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1},
};


// 通过查询机器人板硬件原理图可知，机器人板上电机驱动芯片使用的是L9110S
// 此器件能够驱动一个直流无刷电机，集成了电机正转/反转/停止/刹车四个功能。
// 左右两轮电机各由一个L9110S驱动,GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车

void pwm_contorl(unsigned int  gpio,int speed){
    hi_io_set_func(gpio, GS[gpio].pwm_out);
    IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
    hi_pwm_init(GS[gpio].pwm_port);
    hi_pwm_set_clock(PWM_CLK_160M);
    hi_pwm_start(GS[gpio].pwm_port, speed, PWM_FREQ);
}


// gpio_control用于控制GPIO引脚的输出电平值
void gpio_control (unsigned int  gpio, IotGpioValue value) {
    if(value==IOT_GPIO_VALUE0){
        // hi_io_set_func(gpio, GS[gpio].pwm_out);
        // IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
        // hi_pwm_init(GS[gpio].pwm_port);
        // hi_pwm_set_clock(PWM_CLK_160M);
        // hi_pwm_start(GS[gpio].pwm_port, global_speed, PWM_FREQ);
        pwm_contorl(gpio,global_speed);
    }
    else{
        hi_io_set_func(gpio, GPIOFUNC);
        IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
        IoTGpioSetOutputVal(gpio, value);//设置GPIO引脚的输出电平值。
    }
}

// 小车前进
// GPIO0为高电平，GPIO1为低电平，左轮正转
// GPIO9为高电平，GPIO10为低电平，右轮正转
void car_forward(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 小车后退
// GPIO0为低电平，GPIO1为高电平，左轮反转
// GPIO9为低电平，GPIO10为高电平，右轮反转
void car_backward(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

// 小车左转
// GPIO0和1为低电平,左轮停止
// GPIO9为高电平，GPIO10为低电平，右轮正转
void car_left(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 小车右转
// GPIO0为高电平，GPIO1为低电平，左轮正转
// GPIO9和10为低电平,右轮停止
void car_right(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 用于折算速度，例如，off=600时，电机能量为60%
#define getSpeedOff(off) (global_speed*off/1000+(1000-off))

// 小车停止
// GPIO0和1为高电平,左轮刹车
// GPIO9和10为高电平,右边轮刹车
void car_stop(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

// 小车异侧轮后退左转
void car_back_left(int back_off) {
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    // pwm_contorl(GPIO0,getSpeedOff(back_off));
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

// 小车异侧轮后退右转
void car_back_right(int back_off) {
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    // pwm_contorl(GPIO9,getSpeedOff(back_off));
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 小车异侧轮前进左转
void car_forward_left(int forward_off) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    // gpio_control(GPIO1, IOT_GPIO_VALUE1);
    pwm_contorl(GPIO1,getSpeedOff(forward_off));
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 小车异侧轮前进右转
void car_forward_right(int forward_off) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    // gpio_control(GPIO10, IOT_GPIO_VALUE1);
    pwm_contorl(GPIO10,getSpeedOff(forward_off));
}

// 小车锐角左转
void car_big_left(void) {
    // gpio_control(GPIO0, IOT_GPIO_VALUE0);
    pwm_contorl(GPIO0,getSpeedOff(500));
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}


// 小车锐角右转
void car_big_right(void) {
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    // gpio_control(GPIO9, IOT_GPIO_VALUE0);
    pwm_contorl(GPIO9,getSpeedOff(500));
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

void car_run(CarDir direction){
    if(direction == NULL){
        direction = car_dir_index;
    }
    switch (direction)
    {
    case DIR_FORWARD:car_forward();break;
    case DIR_BACKWARD:car_backward();break;
    case DIR_LEFT:car_left();break;
    case DIR_RIGHT:car_right();break;
    case DIR_STOP:car_stop();break;
    case DIR_BIG_LEFT:car_big_left();break;
    case DIR_BIG_RIGHT:car_big_right();break;
    default:car_stop();break;
    }
}